#include "capture.h"
#include "com_print.h"
#include <stdio.h>





static void print_call_stack(uint32_t *stack_pointer) {
    uint32_t *sp = stack_pointer;  // Get the current stack pointer
    uint32_t return_address;

    COMPrintf("Call Stack Trace:\n");

    // Iterate through the stack and print potential return addresses
    for (int i = 0; i < 20; i++) {  // Maximum depth of 10 frames
        return_address = *(sp++);  // Fetch return address from the stack
        
        
			  if( (return_address > 0x08000000) && (return_address < 0x20000000))
          COMPrintf("Return address %d: 0x%08x\n", i, return_address);
    }
}


void hard_fault_handler_c(uint32_t *hardfault_args, int fault_type) 
{
    uint32_t stacked_r0 = hardfault_args[0];   // R0
    uint32_t stacked_r1 = hardfault_args[1];   // R1
    uint32_t stacked_r2 = hardfault_args[2];   // R2
    uint32_t stacked_r3 = hardfault_args[3];   // R3
    uint32_t stacked_r12 = hardfault_args[4];  // R12
    uint32_t stacked_lr = hardfault_args[5];   // LR (Link register)
    uint32_t stacked_pc = hardfault_args[6];   // PC (Program counter)
    uint32_t stacked_psr = hardfault_args[7];  // xPSR (Program status register)

    // Identify the source of the fault (HardFault or NMI)
    if (fault_type == 0) {
        COMPrintf("Hard Fault occurred!\n");
    } else if (fault_type == 1) {
        COMPrintf("NMI occurred!\n");
    }
		
		COMPrintf("fault_type:%d \r\n", fault_type);

    // Print the register values
    COMPrintf("R0  = 0x%08x\n", stacked_r0);
    COMPrintf("R1  = 0x%08x\n", stacked_r1);
    COMPrintf("R2  = 0x%08x\n", stacked_r2);
    COMPrintf("R3  = 0x%08x\n", stacked_r3);
    COMPrintf("R12 = 0x%08x\n", stacked_r12);
    COMPrintf("LR  = 0x%08x\n", stacked_lr);  // Link register
    COMPrintf("PC  = 0x%08x\n", stacked_pc);  // Program counter
    COMPrintf("PSR = 0x%08x\n", stacked_psr); // Program status register

    // Analyze the value in LR to see the context of the fault
    if ((stacked_lr & 0xF0000000) == 0xF0000000) {
        COMPrintf("LR indicates exception or interrupt return.\n");
    }

    // Output the call stack
    print_call_stack(hardfault_args);

    // Infinite loop to halt the system after a fault
    while(1);
}













